#include "cservowindow.h"

#include <buola/gui/ctoolkit.h>
#include <buola/gui/cicon.h>
#include <buola/gui/cmenu.h>
#include <buola/gui/csimpleapp.h>
#include <buola/io/cfolder.h>
#include <buola/io/cresourcefinder.h>
#include <buola/image/algorithm.h>
#include <buola/image/io.h>
#include <buola/app/ceventloop.h>
#include <buola/mat/transformations.h>
#include <buola/cv/coccupancyoctree.h>
#include <buola/cv/io/ckinect.h>
#include <buola/mat/cgaussianprocess.h>
#include <buola/mat/cmatfile.h>
#include <buola/app/ccmdline.h>
#include <buola/algorithm/comparison.h>
#include <buola/image/point.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/widgets/ctoolbar.h>
#include <buola/threads/thread.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/image/cgc.h>
#include <buola/scene/geodes/cplane.h>
#include <buola/scene/geodes/ccube.h>

using namespace buola;

//more or less like the truth (for default dataset at least)
static mat::CVec3d sStartPosition(0.84,0.09,0.82);
static mat::CQuat_d sStartRotation(mat::C3DRotation(22_deg,79_deg,112_deg));
static mat::CVec3d sCameraOrigin(0,0,0);
static mat::CVec3d sCameraAt(0,0,1);
static mat::CVec3d sCameraUp(0,-1,0);

namespace buola { namespace simulator {

CServoWindow::CServoWindow(const io::CURI &pFolder)
    :   mTimer(200_ms)
{
    io::CFolder lFolder(pFolder);
    
    for(const io::CURI &lSubName : lFolder.GetChildrenURIs())
    {
        io::CFolder lSubFolder(lSubName);
        
        if(!lSubFolder.IsFolder()) continue;
        
        auto lFilenames=lSubFolder.GetChildrenNames(new io::CRegexFileFilter("image_[[:digit:]]*\\.png*"));
        
        if(lFilenames.empty()) continue;

        std::sort(lFilenames.begin(),lFilenames.end());
        
        std::string lFrame=lFilenames.back().substr(6,5);

        SFolder lFolder;
        lFolder.mURI=lSubName;
        lFolder.mFrame=lFrame;
        mFolders.push_back(lFolder);
    }
    
    if(mFolders.empty())
        throw XNotFound("can't find any data folder!\n");
    
    mToolBar=new gui::CToolBar();
    Attach(mToolBar,gui::ATTACH_NORTH,gui::ATTACH_TOOLBAR);

    gui::PMenu lMenu=new gui::CMenu;
    
    mTimer.sTimer.Connect(&CServoWindow::NextIteration,this,true,1);

//    lMenu->Add(gui::new_menu_item(L"reset servo",ICON_STOCK_UP))->
//              eTriggered.Connect(&CServoWindow::ResetServo,this);
    lMenu->Add(gui::new_menu_item(L"next iteration",ICON_STOCK_ARROW_RIGHT))->
              eTriggered.Connect(&CServoWindow::NextIteration,this,true,1);
    lMenu->Add(gui::new_menu_item(L"25 iterations",ICON_STOCK_ARROW_RIGHT_DOUBLE))->
              eTriggered.Connect(&CServoWindow::NextIteration,this,true,25);
//    lMenu->Add(gui::new_menu_item(L"run",ICON_STOCK_RUN))->
//              eTriggered.Connect([this]{mTimer.Toggle();});
    lMenu->Add(gui::new_menu_item(L"save masks (got the name right this time :P)",ICON_STOCK_SAVE))->
              eTriggered.Connect([this]{SaveMask();});
//    lMenu->Add(gui::new_menu_item(L"big run","dialog-ok"))->
//              eTriggered.Connect([this]{BigRun();});
//    lMenu->Add(gui::new_menu_item(L"run","roll"))->
//              eTriggered.Connect(&CServoWindow::ShakeServo,this);
//    lMenu->Add(gui::new_menu_item(L"capture from kinect","camera-web"))->
//              eTriggered.Connect(in_thread([this]{OnCapture();}));
//    lMenu->Add(gui::new_menu_item(L"generate ply",ICON_STOCK_SAVE))->
//              eTriggered.Connect([this]{GeneratePly();});
//    lMenu->Add(gui::new_menu_item(L"generate mask",ICON_STOCK_SAVE))->
//              eTriggered.Connect([this]{GenerateMappableMask();});
    
    mToolBar->SetMenu(lMenu);

    SetupScene();
    SetupJoints(mFolders.front());
    SetupViews();
    SetupInput(mFolders.front());
    AddCommitHook([this](const CRegion &pRegion){OnCommitHook(pRegion);});
}

CServoWindow::~CServoWindow()
{
}

#if 1
void CServoWindow::SetupScene()
{
    mScene=new scene::CScene;
    scene::PRTTransform lVVSTransform=new scene::CRTTransform;
    lVVSTransform->SetTranslation(mat::CVec3d(0,0,0));
    lVVSTransform->SetRotation(mat::CQuat_d());
    mScene->World()->AddChild(lVVSTransform);

    scene::PRTTransform lArmBase=new scene::CRTTransform;
    lVVSTransform->AddChild(lArmBase);

    scene::PRTTransform lHandBase=new scene::CRTTransform;
    lHandBase->SetTranslation(mat::CVec3d(0,-0.040,0));
    lHandBase->SetRotation(mat::CQuat_d(-M_PI_2,mat::CVec3d(0,1,0)));
    mVirtualArm.GetEndEffector()->AddChild(lHandBase);
    mVirtualHand.Update();

    lHandBase->AddChild(mVirtualHand.GetRootTransform());
    
    mVirtualArm.Update();

    lArmBase->AddChild(mVirtualArm.GetRootTransform());

    scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
    lCamera->SetClipping(0.100,10);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    lCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
    lCamera->SetFOV(2*atan2(240,lParams.GetF().y));
    lCamera->LookAt(sCameraAt,sCameraOrigin,sCameraUp);
    
    mVVS.SetScene(mScene,lVVSTransform);
//    mVVS.AddControlledJoint(mVirtualArm.GetJoint(0));
//    mVVS.AddControlledJoint(mVirtualArm.GetJoint(1));
//    mVVS.AddControlledJoint(mVirtualArm.GetJoint(2));
//    mVVS.AddControlledJoint(mVirtualArm.GetJoint(3));
//    mVVS.AddControlledJoint(mVirtualArm.GetJoint(4));
//    mVVS.AddControlledJoint(mVirtualArm.GetJoint(5));
    mVVS.SetCamera(lCamera);
}
#endif

#if 0
void CServoWindow::SetupScene()
{
    mScene=new scene::CScene;
    scene::PRTTransform lVVSTransform=new scene::CRTTransform;
    lVVSTransform->SetTranslation(mat::CVec3d(0,0,0));
    lVVSTransform->SetRotation(mat::CQuat_d());
    mScene->World()->AddChild(lVVSTransform);

    scene::PTransform lNao=scene::CTransform::Import(io::data_finder().FindFile("robot/models/NAO/nao.vrml"));
    lVVSTransform->AddChild(lNao);
    
    lCamera=new scene::CPerspectiveCamera;
    lCamera->SetClipping(0.100,10);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    lCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
    lCamera->SetFOV(2*atan2(240,lParams.GetF().y));
    lCamera->LookAt(sCameraAt,sCameraOrigin,sCameraUp);
    
    mVVS.SetScene(mScene,lVVSTransform);
    mVVS.SetCamera(lCamera);
}
#endif

#if 0
void CServoWindow::SetupScene()
{
    mScene=new scene::CScene;
    scene::PRTTrnasform lVVSTransform=new scene::CRTTransform;
    lVVSTransform->SetTranslation(mat::CVec3d(0,0,0));
    lVVSTransform->SetRotation(mat::CQuat_d());
    mScene->World()->AddChild(lVVSTransform);

    scene::PRTTransform lPlaneTransform=new scene::CRTTransform;
    lVVSTransform->AddChild(lPlaneTransform);
    lPlaneTransform->SetRotation(mat::CQuat_d(90.0_deg,{1,0,0}));

    (new scene::CPlane)->AttachTo(lPlaneTransform);

    
    lCamera=new scene::CPerspectiveCamera;
    lCamera->SetClipping(0.100,10);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    lCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
    lCamera->SetFOV(2*atan2(240,lParams.GetF().y));
    lCamera->LookAt(sCameraAt,sCameraOrigin,sCameraUp);
    
    mVVS.SetScene(mScene,lVVSTransform);
    mVVS.SetCamera(lCamera);
}
#endif

void CServoWindow::SetupJoints(const SFolder &pFolder)
{
    mat::CVecNd<7> lHandPose;
    io::CTextReader lInfoStream(pFolder.mURI/("info_"+pFolder.mFrame+".txt"));
    
    double lDummy;
    
    lInfoStream >> lDummy >> lDummy >> lHandPose;
    
    mVirtualHand.SetPose(lHandPose);
    mVirtualHand.Update();

    mat::CVecNd<6> lArmJoints;
    lInfoStream >> lArmJoints;
    
    robot::kuka::CArmPose lArmPose(lArmJoints);
    
    mVirtualArm.SetPose(lArmPose);
    mVirtualArm.Update();
}

void CServoWindow::SetupViews()
{
    mSceneView=new scene::CSceneView;
    
    mSceneView->AddScene(mScene);
    scene::PScene lScene=new scene::CScene;
    mSceneCloud=new scene::CPointCloud;
    lScene->World()->AddObject(mSceneCloud);
    mSceneView->AddScene(lScene);
    scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
    lCamera->SetFOV(2*atan2(240.0,525.0));
    lCamera->SetClipping(0.010,10);
//        lCamera->SetClipping(0.1,50.0);
//        lCamera->LookAt({0,0,1},{0,0,0},{0,-1,0});
    lCamera->LookAt(sCameraAt,sCameraOrigin,sCameraUp);
    mSceneView->SetCamera(lCamera);
    mSceneView->AddController(new scene::CCameraController(lCamera));
    mSceneView->Resize({640,480});
    mSceneView->SetTitle(L"current iteration");
    
    Add(mSceneView);
    
    mNumbersBox=new gui::CListContainer;
    mNumbersBox->AddGroup(L"pose",0);
    mNumbersBox->AddGroup(L"method",1);
    mNumbersBox->Resize({200,480});
    for(int i=0;i<6;i++)
    {
        mCoordsBox[i]=new gui::CNumBox;
        mCoordsBox[i]->Resize({200,20});
        mNumbersBox->Add(mCoordsBox[i],0);
        if(i<3)
            mCoordsBox[i]->SetStep(0.02);
        else
            mCoordsBox[i]->SetStep(1);
        mCoordsBox[i]->SetStyle(gui::CNumBox::STYLE_DOUBLE);
        mCoordsBox[i]->eNumChange.Connect(&CServoWindow::OnCoordsBox,this,i);
    }
    
    mCoordsBox[0]->SetTitle(L"x");
    mCoordsBox[1]->SetTitle(L"y");
    mCoordsBox[2]->SetTitle(L"z");
    mCoordsBox[3]->SetTitle(L"a");
    mCoordsBox[4]->SetTitle(L"b");
    mCoordsBox[5]->SetTitle(L"c");
    
    m2DBox=new gui::CCheckBox;
    m2DBox->Resize({200,20});
    m2DBox->Check();
    m2DBox->SetTitle(L"2D");
    mNumbersBox->Add(m2DBox,1);
    
    m3DBox=new gui::CCheckBox;
    m3DBox->Resize({200,20});
    m3DBox->Check();
    m3DBox->SetTitle(L"3D");
    mNumbersBox->Add(m3DBox,1);
    
    Add(mNumbersBox);

    mat::CVec3d lPosition=mVVS.Position();
    mat::C3DRotation lRotation(mVVS.Rotation());
    
    mCoordsBox[0]->SetValue(lPosition.x());
    mCoordsBox[1]->SetValue(lPosition.y());
    mCoordsBox[2]->SetValue(lPosition.z());
    mCoordsBox[3]->SetValue(rad2deg(lRotation.a));
    mCoordsBox[4]->SetValue(rad2deg(lRotation.b));
    mCoordsBox[5]->SetValue(rad2deg(lRotation.c));
}

#if 1
void CServoWindow::SetupInput(const SFolder &pFolder)
{
    img::CImage_rgb8 lTargetImage;
    load(lTargetImage,pFolder.mURI/("image_"+pFolder.mFrame+".png"));
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
//    mat::CMatFile lMatFile(mFolder/("kinect/frame_"+mFrame+".mat"));
//    img::CImage_gray32f lDispImage=lMatFile.GetImage("disparity");
    img::CImage_gray32f lDepthImage;
    load_raw(lDepthImage,pFolder.mURI/("disp_"+pFolder.mFrame+".dat"),{640,480});
    disparity_to_depth(lDepthImage,525.0,0.08);
    
    cvi::CCloud_rgb8 lCloud=cvi::make_cloud(lDepthImage,lTargetImage,lParams);

    mSceneCloud->SetCloud(clone(lCloud));
    
    mVVS.SetTarget(lTargetImage,lDepthImage);
    mTargetImage=share(lTargetImage);
    mVVS.SetInitialPose(sStartPosition,sStartRotation);
}
#endif

void CServoWindow::ShakeServo()
{
    mat::CVec3d lCurrentPos=mVVS.Position();
    mat::CQuat_d lCurrentRot=mVVS.Rotation();
    mVVS.SetInitialPose(lCurrentPos+mat::CVec3d(-0.030+0.060*random::get<double>(),-0.030+0.060*random::get<double>(),
                                                -0.030+0.060*random::get<double>()),lCurrentRot);
    Refresh();
}

void CServoWindow::BigRun()
{
    for(int i=0;i<30000;i++)
    {
        int lMode=random::get<int>(2);
        
        mat::CVec3d lPosError(0,0,0);
        mat::CQuat_d lRotError(1,0,0,0);
        if(lMode==0) //only positional error
        {
            double lZError=-0.12+0.24*random::get<double>();
            double lXYError=0.12*random::get<double>();
            double lXError=-1+2*random::get<double>();
            double lYError=-1+2*random::get<double>();
            double lMod=sqrt(sq(lXError)+sq(lYError));
            lXError*=lXYError/lMod;
            lYError*=lXYError/lMod;

            lPosError=mat::CVec3d(lXError,lYError,lZError);
        }
        else if(lMode==1) //only rotational error
        {
            double lAError=-12_deg+24_deg*random::get<double>();
            double lBCError=12_deg*random::get<double>();
            double lBError=-1+2*random::get<double>();
            double lCError=-1+2*random::get<double>();
            double lMod=sqrt(sq(lBError)+sq(lCError));
            lBError*=lBCError/lMod;
            lCError*=lBCError/lMod;
            
            lRotError=mat::CQuat_d(mat::C3DRotation(lAError,lBError,lCError));
        }
        else
        {
            double lXYZError=0.12*random::get<double>();
            double lXError=-1+2*random::get<double>();
            double lYError=-1+2*random::get<double>();
            double lZError=-1+2*random::get<double>();
            double lMod=sqrt(sq(lXError)+sq(lYError)+sq(lZError));
            lXError*=lXYZError/lMod;
            lYError*=lXYZError/lMod;
            lZError*=lXYZError/lMod;

            lPosError=mat::CVec3d(lXError,lYError,lZError);
            
            double lABCError=12_deg*random::get<double>();
            double lAError=-1+2*random::get<double>();
            double lBError=-1+2*random::get<double>();
            double lCError=-1+2*random::get<double>();
            lMod=sqrt(sq(lAError)+sq(lBError)+sq(lCError));

            lAError*=lABCError/lMod;
            lBError*=lABCError/lMod;
            lCError*=lABCError/lMod;
            
            lRotError=mat::CQuat_d(mat::C3DRotation(lAError,lBError,lCError));
        }

        mVVS.SetInitialPose(lPosError+sStartPosition,lRotError*sStartRotation);

        start_timer();
        for(int i=0;i<200;i++)
        {
            mVVS.Iterate(true,true,false);
        }
        end_timer();
        
        msg_info() << "error:" << lPosError.x() << " " << lPosError.y() << " " << lPosError.z() << " " << lRotError << "\n";
        msg_info() << "final:" << mVVS.Position().x() << " " << mVVS.Position().y() << " " << mVVS.Position().z() << " " << mVVS.Rotation() << "\n";
        lPosError=mVVS.Position()-sStartPosition;
        lRotError=mVVS.Rotation()*conj(sStartRotation);
        msg_info() << "diff :" << lPosError.x() << " " << lPosError.y() << " " << lPosError.z() << " " << lRotError << "\n";
    }
}

void CServoWindow::ResetServo()
{
    mVVS.SetInitialPose(sStartPosition,sStartRotation);
    Refresh();
}

void CServoWindow::NextIteration(bool pReally,int pCount)
{
    start_timer();
    for(int i=0;i<pCount;i++)
        mVVS.Iterate(pReally,m2DBox->IsChecked(),m3DBox->IsChecked());
    end_timer();
 
    mDirectionsImage.Recreate(mTargetImage.Size());
    mVVS.GetDirectionsImage(mDirectionsImage);

    auto lInputImage=clone(mTargetImage);
    mVVS.DrawRenderEdges(lInputImage);

    if(!mImageBox[0])
    {
        for(int i=0;i<3;i++)
        {
            mImageBox[i]=new gui::CImageBox;
            mImageBox[i]->SetTitle(L"something");
            Add(mImageBox[i]);
        }

        mImageBox[0]->SetTitle(L"input");
        mImageBox[1]->SetTitle(L"render");
        mImageBox[2]->SetTitle(L"edges");
    }

    mImageBox[0]->SetImage(std::move(lInputImage));
    mImageBox[1]->SetImage(clone(mVVS.LastRender()));
    mImageBox[2]->SetImage(clone(mDirectionsImage));
    mSceneView->Refresh();
    
    mat::CVec3d lPosition=mVVS.Position();
    mat::C3DRotation lRotation(mVVS.Rotation());
    
    mCoordsBox[0]->SetValue(lPosition.x());
    mCoordsBox[1]->SetValue(lPosition.y());
    mCoordsBox[2]->SetValue(lPosition.z());
    mCoordsBox[3]->SetValue(rad2deg(lRotation.a));
    mCoordsBox[4]->SetValue(rad2deg(lRotation.b));
    mCoordsBox[5]->SetValue(rad2deg(lRotation.c));
}

void CServoWindow::OnCoordsBox(int i)
{
    mat::CVec3d lPosition(mCoordsBox[0]->GetDoubleValue(),mCoordsBox[1]->GetDoubleValue(),mCoordsBox[2]->GetDoubleValue());
    mat::C3DRotation lRotation=deg2rad(mat::C3DRotation(mCoordsBox[3]->GetDoubleValue(),mCoordsBox[4]->GetDoubleValue(),mCoordsBox[5]->GetDoubleValue()));
    
    mVVS.SetInitialPose(lPosition,mat::CQuat_d(lRotation));
    NextIteration(false);
    
    mSceneView->Refresh();
}

void CServoWindow::OnCapture()
{
    msg_info() << "capture!!!!\n";
    cvi::CKinect lKinect;
    img::CImage_gray32f lTargetDepth;
    std::tie(mTargetImage,lTargetDepth)=lKinect.GetOne();
    
//     if(!mHaveImages)
//     {
//         scene::PScene lScene=new scene::CScene;
//         scene::PPointCloud lCloud=new scene::CPointCloud(cvi::load_cloud(f));
//         lScene->World()->AddObject(lCloud);
//         mSceneView->AddScene(lScene);
//     }
            
    mVVS.SetTarget(mTargetImage,lTargetDepth);
    
    if(mImageBox[0])
        mImageBox[0]->Refresh();
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    mSceneCloud->SetCloud(cvi::make_cloud(lTargetDepth,mTargetImage,lParams));
    mSceneView->Refresh();
}

void CServoWindow::GeneratePly()
{
    std::vector<mat::CVec3d> lPoints;
    std::vector<mat::CVec3d> lNormals;
    std::vector<int> lFaces;
    mScene->CollectPolygons(lPoints,lNormals,lFaces);
    
    int lNumFaces=0;
    for(int i=0;i<lFaces.size();i++)
    {
        if(lFaces[i]>2) lNumFaces+=lFaces[i]-2;
    }
    
    io::CTextWriter lOutput(io::file("tonto.ply"));
    lOutput << "ply\n";
    lOutput << "format ascii 1.0\n";
    lOutput << "comment Created by buola\n";
    lOutput << "element vertex " << lPoints.size() << "\n";
    lOutput << "property float x\n";
    lOutput << "property float y\n";
    lOutput << "property float z\n";
    lOutput << "property float nx\n";
    lOutput << "property float ny\n";
    lOutput << "property float nz\n";
    lOutput << "element face " << lNumFaces << "\n";
    lOutput << "property list uchar uint vertex_indices\n";
    lOutput << "end_header\n";

    for(int i=0;i<lPoints.size();i++)
    {
        lOutput << lPoints[i].x() << " " << lPoints[i].y() << " " << lPoints[i].z() << " ";
        lOutput << lNormals[i].x() << " " << lNormals[i].y() << " " << lNormals[i].z() << "\n";
    }
    
    int lIndex=0;
    for(int i=0;i<lFaces.size();i++)
    {
        for(int j=2;j<lFaces[i];j++)
        {
            lOutput << "3 " << lIndex+j-2 << " " << lIndex+j-1 << " " << lIndex+j << "\n";
        }
        lIndex+=lFaces[i];
/*        
        lOutput << lFaces[i];
        for(int j=0;j<lFaces[i];j++)
            lOutput << " " << lIndex++;
        lOutput << "\n";
*/
    }
    
    msg_info() << "done!\n";
}

void CServoWindow::GenerateMappableMask()
{
    auto lMask=mVVS.GetMappableMask();
    save(lMask,io::file("mask.pgm"));
}

void CServoWindow::OnCommitHook(const CRegion &pRegion)
{
    return;
    msg_info() << "oncommithook\n";
    CSize_i lSize((int)Size().x/4*4,(int)Size().y/4*4);
    img::CImage_rgb8 lImage(lSize);
     
    start_timer();
    glReadBuffer(GL_FRONT);
    glReadPixels(0,0,lSize.x,lSize.y,GL_RGB,GL_UNSIGNED_BYTE,lImage.RawData());
    glReadBuffer(GL_BACK);
    end_timer();
    
    img::save(lImage,io::file("commit.ppm"));
}

void CServoWindow::SaveMask()
{
    mat::CVec3d lStartPosition=mVVS.Position();
    mat::CQuat_d lStartRotation=mVVS.Rotation();
    
    for(SFolder &lFolder : mFolders)
    {
        msg_info() << lFolder.mURI << "\n";
        SetupJoints(lFolder);
        SetupInput(lFolder);
        mVVS.SetInitialPose(lStartPosition,lStartRotation);
        NextIteration(true,50);
        
        const auto &lMask=mVVS.TransformIndex();
 
        img::CImage_gray8 lMask2(lMask.Size());

        img::CImage_rgb8 lMasked(mTargetImage.Size());
        img::copy_pixels(mTargetImage,lMasked);
        for(int r=0;r<lMask2.Height();r++)
        {
            for(int c=0;c<lMask2.Width();c++)
            {
                if(lMask(c,r)==7||lMask(c,r)==9)
                {
                    lMasked(c,r)=img::pixel(0,255,0);
                    lMask2(c,r)=0;
                }
                else
                {
                    lMask2(c,r)=255;
                }
            }
        }
    
        img::save(lMask2,lFolder.mURI/("handmask_"+lFolder.mFrame+".pgm"));
        img::save(lMasked,lFolder.mURI/("imagemasked_"+lFolder.mFrame+".png"));
        
        mImageBox[0]->SetImage(clone(lMasked));
        mImageBox[0]->Refresh(true);
    }
}

/*namespace simulator*/ } /*namespace buola*/ }

int main(int pNArgs,char **pArgs)
{
    using namespace buola;
    
    buola_init(pNArgs,pArgs);

    try
    {
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();
        io::CURI lFolder;
        if(cmd_line().ArgCount()!=1)
            throw XArgument("wrong number of arguments... just specify the older folder!");
        gui::CSimpleApp<simulator::CServoWindow> lApp(io::arg(0));
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_error() << pE.what() << "\n";
    }

    return buola_finish();
}
